package behavior;

import lejos.nxt.ColorLightSensor;
import lejos.nxt.LightSensor;
import lejos.robotics.navigation.TachoPilot;
import lejos.robotics.subsumption.Behavior;
import java.util.*;

/**
 * This behavior will become active when the sensor detects a white or black
 * underground. The robot will drive a straight line until the underground is
 * the default color. Then, it will decide if it has crossed a barcode, and
 * which one. Finally, it will give instructions to the pilot according o the
 * detected barcode.
 * 
 * The bahavior should be able to detect if a barcode is crossed in reverse. The
 * behavior should be able to align the robot after a barcode has been read.
 */
public class BarcodeReader implements Behavior {

	private LightSensor sensor;
	private TachoPilot pilot;

	private boolean isSupressed = false;
	private int sensorMargin;
	private Hashtable barcodeMap;

	public BarcodeReader(LightSensor sensor, TachoPilot pilot) {
		this.sensor = sensor;
		sensorMargin = 16;
		this.pilot = pilot;
		barcodeMap = new Hashtable();
		setupBarcodes();

	}

	@Override
	public void action() {

		isSupressed = false;
		System.out.println("BarcodeReader");

		pilot.forward();

		/*
		 * try { testReadings(); } catch (InterruptedException e1) { }
		 */

		//TODO: add suppresses in loops
		ArrayList<Integer> readings = getBarcodeReading();		
		//String detectedBarcode = calculateBarcode(readings);
		//Integer barcodeNb = (Integer) barcodeMap.get(detectedBarcode);
		//executeBarcode(barcodeNb);

	}

	@Override
	public void suppress() {
		System.out.println("Barc supp");
		isSupressed = true;
	}

	@Override
	public boolean takeControl() {
		//return getColorFromValue(sensor.readValue()) != 2;
		return true;
	}

	
	
	/**
	 * Returns whether the detected lightvalue is black (0), white(1) or
	 * neutral(2)
	 */
	private int getColorFromValue(int value) {		
		// Detected white
		if (value < 0 + sensorMargin)
			return 1;
		// Detected black
		if (value > 100 - sensorMargin)
			return 0;
		else
			return 2;
	}

	/**
	 * Transforms the data from the lightsensor into strings representing a
	 * barcode. Returns null if the crossed underground wasn't a barcode (ie it
	 * could be a sideline).
	 * 
	 * @throws InterruptedException
	 */
	private ArrayList<Integer> getBarcodeReading(){
		ArrayList<Integer> readings = new ArrayList<Integer>();

		int currentReading = 0;
		int neutralValueCount = 0;
		
		pilot.forward();			

		while (neutralValueCount < 5) {
			currentReading = sensor.readValue();					
			System.out.println(getColorFromValue(currentReading));
			
			if(getColorFromValue(currentReading) == 2) neutralValueCount++;
			else neutralValueCount = 0;
			
			readings.add(currentReading);			
		}
		
		pilot.stop();
		
		//remove the last 5 (neutral) values from the readings
		for (int i = 0; i < 5; i++) {
			readings.remove(readings.size()-1);
		}

		//prepare string with readings to print
		String output = "";
		for (int i = 0; i < readings.size(); i++) {
			output = output + readings.get(i);
		}
		System.out.println(output);	
		
		return readings;
	}

	/**
	 * Returns a string which contains a barcode derived from given readings.
	 * It divides the readings in 7 equal blocks and calculates the mean of each block.
	 */
	private String calculateBarcode(ArrayList<Integer> readings) {
		
		int barlength = Math.round(readings.size() / 7);
		ArrayList<Integer> colors = new ArrayList<Integer>(); 
		
		for (int i = 0; i < 7; i++) {
			
			double tempsum = 0;
			for (int j = 0; j < barlength; j++) {
				tempsum += readings.get(i*barlength + j);
			}
			double meanValue = tempsum / barlength;
			
			colors.add(getColorFromValue((int) meanValue));
		}
		
		//prepare string to return
		String ret = "";
		for (int i = 0; i < colors.size(); i++) {
				ret = ret + colors.get(i);
		}
		System.out.println("detected colors: ");
		System.out.println(ret);
		
		return ret;
	}
	
	/**
	 * Executes the pilot-commands according to given barcodenumber.
	 */
	private void executeBarcode(Integer bcNb)
	{
		if(bcNb == null)
			{System.out.println("Invalid reading!"); return;}
		
		//TODO insert correct values
		int radius = 40;
		int distance = 100;
		switch (bcNb) {

		case 1: // straight, driveforward-behav should become active			
			break;
		case 3: // right turn
			pilot.travelArc(-radius, distance);
			break;
		case 6:  // left turn;
			pilot.travelArc(radius, distance); 
			break;
		default:
			System.out.println("Detected bc doesnt respond to an action");
		}	
	}

	/**
	 * Adds barcode-data to the barcodeMap See the online pdf for barcode
	 * numbers
	 */
	private void setupBarcodes() {
		barcodeMap.put("0001111", 1); // straight
		barcodeMap.put("0110011", 6); // right turn
		barcodeMap.put("0011001", 3); // left turn
	}

	private void testReadings() throws InterruptedException {

		System.out.println("executing testreadings");
		while (true) {
			
			int reading = sensor.readValue();
			System.out.println(getColorFromValue(reading) + " fake");
			System.out.println(reading + " real");

			Thread.sleep(3000);
		}
	}

}
